#!/usr/bin/python

import roslib; roslib.load_manifest('framer')
import rospy,smach,tf
from sensor_msgs.msg import Image, CameraInfo
import image_geometry
from geometry_msgs.msg import PointStamped
from math import sqrt

class CenterRule(smach.State):
    def __init__(self):
        smach.State.__init__(
            self,
            outcomes=['fail','abort','success'],
            output_keys=['picture_center', 'cam_model'],
            input_keys=['faces', 'cam_model']
        )
        self.tf = tf.TransformListener()

    def execute(self, data):
        if rospy.is_shutdown():
            return 'abort'
        if not data.faces:
            rospy.logerr(
                "Called to frame faces, but received NoneType for faces")
            return 'fail'
        if len(data.faces.faces) == 0:
            return 'fail'
        facePoints = [face.center for face in data.faces.faces]
        closestFace = self.findClosestFace(data.cam_model, facePoints)
        self.removeSomeHeadRoom(closestFace, data.faces.header.frame_id)
        center = PointStamped()
        center.point = closestFace
        center.header = data.faces.header
        data.picture_center = center
        return 'success'

    def removeSomeHeadRoom(self, point, frame_id):
        ps = PointStamped()
        ps.point = point
        ps.header.frame_id = frame_id
        transformed = self.tf.transformPoint('/head_tilt_link', ps)
        point.z -= sqrt(transformed.point.y**2+transformed.point.x**2)/sqrt(3)
    def findClosestFace(self, cam_model, facePoints):
        closestFace = None
        shortestDistance = float('inf')
        (cu, cv) = (cam_model.cx(), cam_model.cy())
        for face in facePoints:
            (u,v) = cam_model.project3dToPixel((face.x, face.y, face.z))
            dist = sqrt((u-cu)**2+(v-cv)**2)
            if shortestDistance > dist:
                shortestDistance = dist
                closestFace = face
        return face


